package ballFinder.behavior;

import ballFinder.environment.Line;
import ballFinder.environment.MotorControl;
import ballFinder.environment.SensorControl;
import ballFinder.environment.World;
import lejos.nxt.Sound;
import lejos.robotics.subsumption.Behavior;

public class StopInFrontOfBall implements Behavior {

	private boolean suppressAction;
	private int US_MIN_RANGE = 25;
	
	private SensorControl sensors; 
	private MotorControl motors;
	private World world;
	
	public StopInFrontOfBall(SensorControl sensors, MotorControl motors, World world){
		this.sensors = sensors;
		this.motors = motors;
		this.world = world;
	}
	
	@Override
	public void action() {
		suppressAction = false;
		motors.stop();
		beep();
		printPosition();
		while(!suppressAction){
			Thread.yield();
		}
	}

	private void printPosition() {
		Line currentPosition = world.getCurrentPosition();
		double distance = world.getDirectDistance();
		double drivenDistance = world.getDrivenDistance();
		System.out.println("Aktuelle Position: " + (int)currentPosition.x1 + " " + (int)currentPosition.y1);
		System.out.println("Distanz zum Startpunkt: " + (int)distance);
		System.out.println("Gefahrene Strecke: " + (int)drivenDistance);
	}

	private void beep() {
		tetris();
		Sound.setVolume(0);
	}

	@Override
	public void suppress() {
		suppressAction = true;
	}

	@Override
	public boolean takeControl() {
		if((sensors.getInfraredDirection() == SensorControl.Direction.InFront) &&
			(sensors.getUSDistance() < US_MIN_RANGE)){
			return true;
		}
		return false;
	}
	
	static void tetris(){
		int a6 = 1760;
		int ar6 = 1864;
		int c7 = 2093;
		int d6 = 1174;
		int d7 = 2349;
		int e6 = 1316;
		int f6 = 1396;
		int g6 = 1567;
		Sound.setVolume(100);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(ar6,150);  wait10Msec(15);
		Sound.playTone(d7,150);  wait10Msec(15);
		Sound.playTone(d7,150);  wait10Msec(15);
		Sound.playTone(c7,150);  wait10Msec(15);
		Sound.playTone(ar6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(e6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(g6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(a6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(f6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);
		Sound.playTone(d6,150);  wait10Msec(15);	
		}
	
	private static void wait10Msec(int i) {
		try {
			Thread.sleep(10*i);
		} catch (InterruptedException e) {}
		
	}

}
